db-A*: Discontinuity-bounded Search for Kinodynamic Mobile Robot Motion Planning

We consider time-optimal motion planning for dynamical systems that are translation-invariant, a property that holds for many mobile robots, such as differential-drives, cars, airplanes, and multirotors. Our key insight is that we can extend graph-search algorithms to the continuous case when used symbiotically with optimization. For the graph search, we introduce discontinuity-bounded A* (db-A*), a generalization of the A* algorithm that uses concepts and data structures from sampling-based planners. Db-A* reuses short trajectories, so-called motion primitives, as edges and allows a maximum user-specified discontinuity at the vertices. These trajectories are locally repaired with trajectory optimization, which also provides new improved motion primitives. Our novel kinodynamic motion planner, kMP-db-A*, has almost surely asymptotic optimal behavior and computes near-optimal solutions quickly. For our empirical validation, we provide the first benchmark that compares search-, sampling-, and optimization-based time-optimal motion planning on multiple dynamical systems in different settings. Compared to the baselines, kMP-db-A* consistently solves more problem instances, finds lower-cost initial solutions, and converges more quickly.Comment: Accepted at IROS 202

Parallel Tracking and Mapping algorithms for an Event Based Camera

An event camera has independent pixels that sends information, called “events” when they perceive a local change of brightness. The information is transmitted asynchronously exactly when the change occurs, with a microsecond resolution, making this sensor suitable for fast robotics applications. We present two new tracking and mapping algorithms, designed to work in parallel to estimate the 6 DOF (Degrees Of Freedom) trajectory and the structure of the scene in line based environments. The tracking thread is based on a Landmark Based map and an asynchronous EKF (Extended Kalman Filter) filter to estimate event per event the state of the camera unlocking the true potential of the camera. Inside the mapping thread, a line extraction algorithm has been designed to find 3D segments in the Point cloud, computed using event – ray tracing into a discretized world. Both algorithms have been built from scratch, and at this moment, only tested independently in simulation. We have obtained very good results on three synthetic self-made datasets. Some pieces of the complete Parallel Tracking and Mapping system are still missing. The current good work and results encourages to improve and finish the algorithm to achieve the implementation on the real event based camera

db-CBS: Discontinuity-Bounded Conflict-Based Search for Multi-Robot Kinodynamic Motion Planning

This paper presents a multi-robot kinodynamic motion planner that enables a team of robots with different dynamics, actuation limits, and shapes to reach their goals in challenging environments. We solve this problem by combining Conflict-Based Search (CBS), a multi-agent path finding method, and discontinuity-bounded A*, a single-robot kinodynamic motion planner. Our method, db-CBS, operates in three levels. Initially, we compute trajectories for individual robots using a graph search that allows bounded discontinuities between precomputed motion primitives. The second level identifies inter-robot collisions and resolves them by imposing constraints on the first level. The third and final level uses the resulting solution with discontinuities as an initial guess for a joint space trajectory optimization. The procedure is repeated with a reduced discontinuity bound. Our approach is anytime, probabilistically complete, asymptotically optimal, and finds near-optimal solutions quickly. Experimental results with robot dynamics such as unicycle, double integrator, and car with trailer in different settings show that our method is capable of solving challenging tasks with a higher success rate and lower cost than the existing state-of-the-art.Comment: submitted to ICRA 202

A Conflict-driven Interface between Symbolic Planning and Nonlinear Constraint Solving

Robotic planning in real-world scenarios typically requires joint optimization of logic and continuous variables. A core challenge to combine the strengths of logic planners and continuous solvers is the design of an efficient interface that informs the logical search about continuous infeasibilities. In this paper we present a novel iterative algorithm that connects logic planning with nonlinear optimization through a bidirectional interface, achieved by the detection of minimal subsets of nonlinear constraints that are infeasible. The algorithm continuously builds a database of graphs that represent (in)feasible subsets of continuous variables and constraints, and encodes this knowledge in the logical description. As a foundation for this algorithm, we introduce Planning with Nonlinear Transition Constraints (PNTC), a novel planning formulation that clarifies the exact assumptions our algorithm requires and can be applied to model Task and Motion Planning (TAMP) efficiently. Our experimental results show that our framework significantly outperforms alternative optimization-based approaches for TAMP

Neural Field Representations of Articulated Objects for Robotic Manipulation Planning

Traditional approaches for manipulation planning rely on an explicit geometric model of the environment to formulate a given task as an optimization problem. However, inferring an accurate model from raw sensor input is a hard problem in itself, in particular for articulated objects (e.g., closets, drawers). In this paper, we propose a Neural Field Representation (NFR) of articulated objects that enables manipulation planning directly from images. Specifically, after taking a few pictures of a new articulated object, we can forward simulate its possible movements, and, therefore, use this neural model directly for planning with trajectory optimization. Additionally, this representation can be used for shape reconstruction, semantic segmentation and image rendering, which provides a strong supervision signal during training and generalization. We show that our model, which was trained only on synthetic images, is able to extract a meaningful representation for unseen objects of the same class, both in simulation and with real images. Furthermore, we demonstrate that the representation enables robotic manipulation of an articulated object in the real world directly from images

BITKOMO: Combining Sampling and Optimization for Fast Convergence in Optimal Motion Planning

Optimal sampling based motion planning and trajectory optimization are two competing frameworks to generate optimal motion plans. Both frameworks have complementary properties: Sampling based planners are typically slow to converge, but provide optimality guarantees. Trajectory optimizers, however, are typically fast to converge, but do not provide global optimality guarantees in nonconvex problems, e.g. scenarios with obstacles. To achieve the best of both worlds, we introduce a new planner, BITKOMO, which integrates the asymptotically optimal Batch Informed Trees (BIT*) planner with the K-Order Markov Optimization (KOMO) trajectory optimization framework. Our planner is anytime and maintains the same asymptotic optimality guarantees provided by BIT*, while also exploiting the fast convergence of the KOMO trajectory optimizer. We experimentally evaluate our planner on manipulation scenarios that involve high dimensional configuration spaces, with up to two 7-DoF manipulators, obstacles and narrow passages. BITKOMO performs better than KOMO by succeeding even when KOMO fails, and it outperforms BIT* in terms of convergence to the optimal solution.Comment: 6 pages, Accepted at IROS 202

Simultaneous trajectory and contact optimization with an augmented Lagrangian algorithm

National audienceLegged robots do not require a flat and smooth surface to advance and have, therefore, thepotential to work in unstructured and complex environments. These robots, like humansand animals, can climb stairs and ladders and cross challenging terrain, which is essentialfor exploration and rescue applications.However, walking is a complex task. The robot movement must be a consequence of creat-ing contacts with the environment. Every time a new contact is made the dynamics becomediscontinuous. Also, legged robots are unstable and subject to many constraints, which re-stricts the potential movements. Finally, contact planning has a combinatorial structure: therobot chooses the optimal subset of contacts out of an infinite number of possible contactpoints.Trajectory optimization in robotics is often decoupled into two independent modules. Aproblem formulation, that models the robot motion and transforms it into a mathematicaloptimization problem, and the resolution of this optimization problem with a suitable op-timization solver. We rather envision this question as a two-way interaction between thetwo modules, that can not be considered separately. Indeed, the robotics problem formu-lation must also be a consequence of the understanding of the capabilities of the numericaloptimization algorithm.We propose a simultaneous trajectory and contact optimization with an augmented La-grangian algorithm. The contacts with the environment are modeled in a continuous waywith complementarity constraints. For example, the normal contact model is defined bytwo inequalities: positive force and distance to the surface, and a complementarity relation:either the force or the distance to the surface is zero.These constraints are added as path constraints in a continuous optimal control formula-tion. Using a direct collocation, the formulation is converted into a non-linear constrainedoptimization problem. Optimization problems with complementarity constraints have a de-generated and challenging structure and do not fulfil basic constraints qualifications usedby standard numerical solvers. Therefore, we propose to solve it using an augmented La-grangian algorithm, which offers a good behaviour under this type of constraints and canbe efficiently warmstarted.In this thesis we present our preliminary results with a robot manipulator interacting withthe environment. Optimizing simultaneously trajectory and contacts, we compute interest-ing motions such as multiple surface touching and pick and place tasks. We also outline ourfuture plans to generate walking motions with legged robots

Parallel Tracking and Mapping algorithms for an Event Based Camera

An event camera has independent pixels that sends information, called “events” when they perceive a local change of brightness. The information is transmitted asynchronously exactly when the change occurs, with a microsecond resolution, making this sensor suitable for fast robotics applications. We present two new tracking and mapping algorithms, designed to work in parallel to estimate the 6 DOF (Degrees Of Freedom) trajectory and the structure of the scene in line based environments. The tracking thread is based on a Landmark Based map and an asynchronous EKF (Extended Kalman Filter) filter to estimate event per event the state of the camera unlocking the true potential of the camera. Inside the mapping thread, a line extraction algorithm has been designed to find 3D segments in the Point cloud, computed using event – ray tracing into a discretized world. Both algorithms have been built from scratch, and at this moment, only tested independently in simulation. We have obtained very good results on three synthetic self-made datasets. Some pieces of the complete Parallel Tracking and Mapping system are still missing. The current good work and results encourages to improve and finish the algorithm to achieve the implementation on the real event based camera

Conflict-Directed Diverse Planning for Logic-Geometric Programming

Robots operating in the real world must combine task planning for reasoning about what to do with motion planning for reasoning about how to do it -- this is known as task and motion planning. One promising approach for task and motion planning is Logic Geometric Programming (LGP) which integrates a logical layer and a geometric layer in an optimization formulation. The logical layer describes feasible high-level actions at an abstract symbolic level, while the geometric layer uses continuous optimization methods to reason about motion trajectories with geometric constraints. In this paper we propose a new approach for solving task and motion planning problems in the LGP formulation, that leverages state-of-the-art diverse planning at the logical layer to explore the space of feasible logical plans, and minimizes the number of optimization problems to be solved on the continuous geometric layer. To this end, geometric infeasibility is fed back into planning by identifying prefix conflicts and incorporating this back into the planner through a novel multi-prefix forbidding compilation. We further leverage diverse planning with a new novelty criteria for selecting candidate plans based on the prefix novelty, and a metareasoning approach which attempts to extract only useful conflicts by leveraging the information that is gathered in the course of solving the given problem